#include <GameEngine/UnitTest/unit_test.h>

#include <GameEngine/AI/Navmesh/funnel.h>
#include <GameEngine/Core/timer.h>


#ifdef GAMEENGINE_NAMESPACES
namespace GameEngine {
#endif


//! Launch the unit tests on graph lists
/*!
 @return True if all unit tests on graph lists were
         succesful, false otherwise
 */
bool UnitTest::test_ai_funnel_functions() {
	bool res = true;

	// Get number of bytes
	MemoryManager::BytesCount bytes = MemoryManager::instance()->bytes();

	{{
		// Create funnel points
		std::vector< GameEngine::Point2d<float> > pts;
		pts.push_back( GameEngine::Point2d<float>(0,0) );
		pts.push_back( GameEngine::Point2d<float>(1,0) );
		pts.push_back( GameEngine::Point2d<float>(0,1) );
		pts.push_back( GameEngine::Point2d<float>(1,1) );
		pts.push_back( GameEngine::Point2d<float>(2,1) );
		pts.push_back( GameEngine::Point2d<float>(2,2) );
		pts.push_back( GameEngine::Point2d<float>(1,2) );
		pts.push_back( GameEngine::Point2d<float>(2,3) );
		GameEngine::Point2d<float>* p0 = &(pts[0]); // p0
		GameEngine::Point2d<float>* p1 = &(pts[1]); // p1
		GameEngine::Point2d<float>* p2 = &(pts[2]); // p2
		GameEngine::Point2d<float>* p3 = &(pts[3]); // p3
		GameEngine::Point2d<float>* p4 = &(pts[4]); // p4
		GameEngine::Point2d<float>* p5 = &(pts[5]); // p5
		GameEngine::Point2d<float>* p6 = &(pts[6]); // p6
		GameEngine::Point2d<float>* p7 = &(pts[7]); // p7

		// Create funnel triangles
		std::vector< GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl > trgls;
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p0,p1,p2)); // t0
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p2,p3)); // t1
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p3,p4)); // t2
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p3,p4,p6)); // t3
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p4,p5,p6)); // t4
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p5,p6,p7)); // t5

		// Test shortest path
		std::list< GameEngine::Point2d<float> > path = GameEngine::Funnel< GameEngine::Point2d<float> >::find_path(trgls);
		res &= UNIT_TEST_ERROR( "[Funnel] find_path function", ( path.size() == 3 ) );
		float tolerance = 0.000001f;
		if ( path.size() == 3 ) {
			std::list< GameEngine::Point2d<float> >::iterator it = path.begin();
			res &= UNIT_TEST_ERROR( "[Funnel] find_path function", ( it->get_distance(*p0) < tolerance ) );
			++it;
			res &= UNIT_TEST_ERROR( "[Funnel] find_path function", ( it->get_distance(*p3) < tolerance ) );
			++it;
			res &= UNIT_TEST_ERROR( "[Funnel] find_path function", ( it->get_distance(*p7) < tolerance ) );
		}

		// Setup benchmarks
		#ifdef GAMEENGINE_BENCHMARKS
		{{
			GameEngine::Timer timer;
			int nb_max = 1000;
			for ( int k = 0; k < nb_max; ++k ) {
				std::list< GameEngine::Point2d<float> > path = GameEngine::Funnel< GameEngine::Point2d<float> >::find_path(trgls);
			}
			double t = 1000.0*timer.elapsed_time();
			std::cout << "[Funnel] find_path() time = " << (t/(float)nb_max) << " ms for " << trgls.size()
				<< " triangles = " << (t/((float)nb_max*trgls.size())) << "/triangle." << std::endl;
		}}
		#endif
	}}

	{{
		// Create funnel points
		std::vector< GameEngine::Point2d<float> > pts;
		pts.push_back( GameEngine::Point2d<float>(0,0) );
		pts.push_back( GameEngine::Point2d<float>(1,0) );
		pts.push_back( GameEngine::Point2d<float>(0,1) );
		pts.push_back( GameEngine::Point2d<float>(-0.5,2) );
		pts.push_back( GameEngine::Point2d<float>(1,1) );
		pts.push_back( GameEngine::Point2d<float>(2,1) );
		pts.push_back( GameEngine::Point2d<float>(2,0) );
		pts.push_back( GameEngine::Point2d<float>(2,-1) );
		pts.push_back( GameEngine::Point2d<float>(1,-1) );
		GameEngine::Point2d<float>* p0 = &(pts[0]); // p0
		GameEngine::Point2d<float>* p1 = &(pts[1]); // p1
		GameEngine::Point2d<float>* p2 = &(pts[2]); // p2
		GameEngine::Point2d<float>* p3 = &(pts[3]); // p3
		GameEngine::Point2d<float>* p4 = &(pts[4]); // p4
		GameEngine::Point2d<float>* p5 = &(pts[5]); // p5
		GameEngine::Point2d<float>* p6 = &(pts[6]); // p6
		GameEngine::Point2d<float>* p7 = &(pts[7]); // p7
		GameEngine::Point2d<float>* p8 = &(pts[8]); // p8

		// Create funnel triangles
		std::vector< GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl > trgls;
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p0,p1,p2)); // t0
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p2,p3)); // t1
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p3,p4)); // t2
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p4,p5)); // t3
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p5,p6)); // t4
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p6,p7)); // t5
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p7,p8)); // t6

		// Test shortest path
		std::list< GameEngine::Point2d<float> > path = GameEngine::Funnel< GameEngine::Point2d<float> >::find_path(trgls);
		res &= UNIT_TEST_ERROR( "[Funnel] find_path function", ( path.size() == 3 ) );
		float tolerance = 0.000001f;
		if ( path.size() == 3 ) {
			std::list< GameEngine::Point2d<float> >::iterator it = path.begin();
			res &= UNIT_TEST_ERROR( "[Funnel] find_path function", ( it->get_distance(*p0) < tolerance ) );
			++it;
			res &= UNIT_TEST_ERROR( "[Funnel] find_path function", ( it->get_distance(*p1) < tolerance ) );
			++it;
			res &= UNIT_TEST_ERROR( "[Funnel] find_path function", ( it->get_distance(*p8) < tolerance ) );
		}

		// Setup benchmarks
		#ifdef GAMEENGINE_BENCHMARKS
		{{
			GameEngine::Timer timer;
			int nb_max = 1000;
			for ( int k = 0; k < nb_max; ++k ) {
				std::list< GameEngine::Point2d<float> > path = GameEngine::Funnel< GameEngine::Point2d<float> >::find_path(trgls);
			}
			double t = 1000.0*timer.elapsed_time();
			std::cout << "[Funnel] find_path() time = " << (t/(float)nb_max) << " ms for " << trgls.size()
				<< " triangles = " << (t/((float)nb_max*trgls.size())) << "/triangle." << std::endl;
		}}
		#endif
	}}

	{{
		// Create funnel points
		std::vector< GameEngine::Point2d<float> > pts;
		pts.push_back( GameEngine::Point2d<float>(0,0) );
		pts.push_back( GameEngine::Point2d<float>(1,0) );
		pts.push_back( GameEngine::Point2d<float>(0,1) );
		pts.push_back( GameEngine::Point2d<float>(-0.5,2) );
		pts.push_back( GameEngine::Point2d<float>(1,1) );
		pts.push_back( GameEngine::Point2d<float>(2,1) );
		pts.push_back( GameEngine::Point2d<float>(4,0.5) );
		pts.push_back( GameEngine::Point2d<float>(2,0) );
		pts.push_back( GameEngine::Point2d<float>(2,-1) );
		pts.push_back( GameEngine::Point2d<float>(1,-1) );
		GameEngine::Point2d<float>* p0 = &(pts[0]); // p0
		GameEngine::Point2d<float>* p1 = &(pts[1]); // p1
		GameEngine::Point2d<float>* p2 = &(pts[2]); // p2
		GameEngine::Point2d<float>* p3 = &(pts[3]); // p3
		GameEngine::Point2d<float>* p4 = &(pts[4]); // p4
		GameEngine::Point2d<float>* p5 = &(pts[5]); // p5
		GameEngine::Point2d<float>* p6 = &(pts[6]); // p6
		GameEngine::Point2d<float>* p7 = &(pts[7]); // p7
		GameEngine::Point2d<float>* p8 = &(pts[8]); // p8
		GameEngine::Point2d<float>* p9 = &(pts[9]); // p9

		// Create funnel triangles
		std::vector< GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl > trgls;
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p0,p1,p2)); // t0
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p2,p3)); // t1
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p3,p4)); // t2
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p4,p5)); // t3
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p5,p6)); // t4
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p6,p7)); // t5
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p7,p8)); // t6
		trgls.push_back(GameEngine::Funnel< GameEngine::Point2d<float> >::Trgl(p1,p8,p9)); // t7

		// Test shortest path
		std::list< GameEngine::Point2d<float> > path = GameEngine::Funnel< GameEngine::Point2d<float> >::find_path(trgls);
		res &= UNIT_TEST_ERROR( "[Funnel] find_path function", ( path.size() == 3 ) );
		float tolerance = 0.000001f;
		if ( path.size() == 3 ) {
			std::list< GameEngine::Point2d<float> >::iterator it = path.begin();
			res &= UNIT_TEST_ERROR( "[Funnel] find_path function", ( it->get_distance(*p0) < tolerance ) );
			++it;
			res &= UNIT_TEST_ERROR( "[Funnel] find_path function", ( it->get_distance(*p1) < tolerance ) );
			++it;
			res &= UNIT_TEST_ERROR( "[Funnel] find_path function", ( it->get_distance(*p9) < tolerance ) );
		}

		// Setup benchmarks
		#ifdef GAMEENGINE_BENCHMARKS
		{{
			GameEngine::Timer timer;
			int nb_max = 1000;
			for ( int k = 0; k < nb_max; ++k ) {
				std::list< GameEngine::Point2d<float> > path = GameEngine::Funnel< GameEngine::Point2d<float> >::find_path(trgls);
			}
			double t = 1000.0*timer.elapsed_time();
			std::cout << "[Funnel] find_path() time = " << (t/(float)nb_max) << " ms for " << trgls.size()
				<< " triangles = " << (t/((float)nb_max*trgls.size())) << "/triangle." << std::endl;
		}}
		#endif
	}}

	// Get number of bytes
	res &= UNIT_TEST_ERROR( "[Funnel] Test memory leak", ( bytes == MemoryManager::instance()->bytes() ));

	// Return result
	return res;
}


#ifdef GAMEENGINE_NAMESPACES
} // namespace GameEngine
#endif
